//
//  Marker.cpp
//  newAR
//
//  Created by ZJU_CVG on 15/1/29.
//  Copyright (c) 2015年 Shangjin Zhai. All rights reserved.
//

#include "Marker.h"
#include "Solve3D.h"
#include "global.h"
namespace markerAR
{
    
Marker::Marker()
{
    registed = false;
    markerNow = nullptr;
}

Marker::~Marker()
{
    for (int i=0; i<this->img.rows-1; ++i) {
        delete[] Coeffs[i];
    }
    delete[] Coeffs;
    delete orbStructure;
    delete multiBrief;
}

void Marker::buildImages(const Mat& img)
{
    
}


//void Marker::optimiseRt(const cv::Mat &nowImg, const IntrinsicMatrix &K, Pose &C_, const vector<Point3f> &pts3d, const vector<Point2f> &pts2d, const cv::Mat &gx, const cv::Mat &gy, double w[3])const
//{
//    CV_Assert(pts3d.size()==pts2d.size());
////    rt.optimise(nowImg, K, C_, pts3d, pts2d, gx, gy, w);
//}

Marker::Marker(const Mat &img,const Eigen::Matrix3d& R,int id,const double scale,int levelNum,float scaleFactor)
{
    this->scaleFactor = scaleFactor;
    Mat grey;
    if(img.type() != CV_8U)
        cvtColor(img, grey, CV_BGR2GRAY);
    else
        grey = img;
    this->img = grey.clone();
    _id = id;
    multiBrief = new MultiScaleStructure(grey, scaleFactor, levelNum);
    orbStructure = new SingleScaleStructrue();
//    brief->detect(multiBrief->allImgs, multiBrief->allKpts,1000,1.2,8);
//    brief->compute(multiBrief->allImgs, multiBrief->allKpts, multiBrief->allDesp);
    
    brief->detect(multiBrief->allImgs,multiBrief->allFts, 1000, 1.2, 8);
    brief->compute(multiBrief->allImgs, multiBrief->allFts);
    Solve3D::get3DPoints(K->K, R, multiBrief->allFts, C.t, multiBrief->scale, multiBrief->levelNum);
    
    myorb->detect(img,orbStructure->kpts);
	myorb->compute(img, orbStructure->kpts, orbStructure->desp);
    
    this->keyframe =  std::make_shared<Keyframe>();
    this->keyframe->feature = *orbStructure;
    this->keyframe->img = grey.clone();
    this->keyframe->setSmallBlurImage();
    this->keyframe->markers.push_back(this);
    this->keyframe->Hkm.push_back(Eigen::Matrix3d().setIdentity());
    Solve3D::get3DPoints(K->K, R, orbStructure->kpts, orbStructure->p3d,C.t);
    cout<<orbStructure->kpts.size()<<endl;
    cout<<orbStructure->p3d[0]<<endl;
    this->C.R = R;

    Mat scaleImg;
    resize(grey, scaleImg, cv::Size(scale*grey.rows,scale*grey.cols));
    scaleK = K->K;
    scaleK = scaleK*scale;
    scaleK(2,2) = 1;

    cv::KeyPoint k;
    vector<KeyPoint> ks;
    vector<Point3f> xyzs;
    k.pt.x = img.cols/2;
    k.pt.y = img.rows/2;
    ks.push_back(k);

    Eigen::Vector3d t;
    Solve3D::get3DPoints(K->K, R, ks, xyzs, t);
    
    
    auto a = xyzs[0];
    center.x = a.x;
    center.y = a.y;
    center.z = a.z;
    
    //Coeffs for bilinear intepolation
    Coeffs = new CoeffBlock*[img.rows-1];
    for (int i=0; i<this->img.rows-1; ++i) {
        Coeffs[i] = new CoeffBlock[img.cols];
        for (int j=0; j<this->img.cols-1; ++j) {
            int a00 = this->img.at<uchar>(i,j);
            int a10 = this->img.at<uchar>(i+1,j);
            int a01 = this->img.at<uchar>(i,j+1);
            int a11 = this->img.at<uchar>(i+1,j+1);
            Coeffs[i][j].coeff[0] = a00;
            Coeffs[i][j].coeff[1] = a10-a00;
            Coeffs[i][j].coeff[2] = a01-a00;
            Coeffs[i][j].coeff[3] = a11+a00-a10-a01;
        }
    }

}



class parallelWarp : public ParallelLoopBody//参考官方给出的answer，构造一个并行的循环体类
{
public:
    parallelWarp(const Eigen::Matrix3d &invH,CoeffBlock**coeffs,const cv::Mat& mask, cv::Mat&warpImg)//class constructor
    {
        this->invH = invH;
        this->coeffs = coeffs;
        this->warpImg = &warpImg;
        this->mask = mask;
        rows = warpImg.rows;
        cols = warpImg.cols;
    }
    void operator()(const Range& range)const//重载操作符（）
    {
        float x2,y2;
        int floorx,floory;
        float resx,resy;
        for (int i = range.start; i < range.end; i++)
        {
            uchar * p = warpImg->ptr<uchar>(i);
            for (int j=0; j<cols; ++j) {
                
                if (mask.ptr<uchar>(i)[j]) {
                    
                    double x3 = invH(2,0)*j+invH(2,1)*i+invH(2,2);
                    x2 = (invH(0,0)*j+invH(0,1)*i+invH(0,2))/x3;
                    y2 = (invH(1,0)*j+invH(1,1)*i+invH(1,2))/x3;
                    if(x2<0||x2>=cols-2||y2<0||y2>=rows-2)
                        continue;
                    floorx = std::floor(x2);
                    floory = std::floor(y2);
                    resx = x2-floorx;
                    resy = y2-floory;
                    CoeffBlock&b = coeffs[floory][floorx];
                    p[j] =b.coeff[0]+b.coeff[1]*resx+(b.coeff[2]+b.coeff[3]*resx)*resy;
                }
            }
        }
    }
    
private:
    Eigen::Matrix3d invH;
    CoeffBlock** coeffs;
    cv::Mat* warpImg;
    cv::Mat mask;
    int cols,rows;
};


Eigen::Matrix3d Marker::warpScale(const Eigen::Matrix3d &Hnm,const cv::Mat&mask,cv::Mat&warpImg,int level)const
{
    warpTo(Hnm, mask, warpImg);
    
    double scale = 1 / pow(scaleFactor, level);
    Eigen::Matrix3d s =Eigen::Matrix3d::Identity();
    s(0, 0) = scale;
    s(1, 1) = scale;
    s = s*Hnm.inverse();
    return s.inverse();
}


void Marker::warpTo(const Eigen::Matrix3d &Hnm,const cv::Mat &mask,cv::Mat &warpImg)const
{
    auto Hmn = Hnm.inverse();
    warpImg = Mat::zeros(mask.size(), CV_8U);
    parallel_for_(cv::Range(0,warpImg.rows), parallelWarp(Hmn, Coeffs, mask, warpImg));

}


void Marker::reset()
{
}
}
